
#ifndef PX_PLANE_H
#define PX_PLANE_H

#include <pxVector3.h>

namespace Px {
namespace Geometry {

/* Standard Geometric Plane: a*x + b*y + c*z + d = 0 */
template <typename Real>
class Plane {
    using Vector3 = Math::Vector3<Real>;
    
public:
    Plane();
    Plane(const Plane& plane);
    Plane(const Vector3& normal, Real d);
    Plane(Real a, Real b, Real c, Real d);
    Plane(const Vector3& point, const Vector3& normal);
    Plane(const Vector3& a, const Vector3& b, const Vector3& c);
    virtual ~Plane();

    void setA(Real a);
    void setB(Real b);
    void setC(Real c);
    void setD(Real d);
    void setNormal(const Vector3& normal);
    void set(const Vector3& normal, const Vector3& point);
    void set(Real nx, Real ny, Real nz, Real x, Real y, Real z);
    
    /* Returns the normalized normal (a, b, c) of this plane. */
    Vector3 normal() const;
    Vector3 origin() const;
    Vector3 getNormal() const;
    Vector3 getOrigin() const;

    bool valid() const;
    bool degenerate() const;

    static Vector3 Normal(const Vector3& a, const Vector3& b, const Vector3& c);
    static bool IntersectSphere(const Plane& plane, const Vector3& center, Real radius);
    static bool IntersectAABB(const Plane& plane, const Vector3& min, const Vector3& max);
    static bool IntersectSegment(const Plane& plane, const Vector3& a, const Vector3& b, Real& t, Vector3& q);
    static Real Distance(const Plane& plane, const Vector3& point);
    static Real UnsignedDistance(const Plane& plane, const Vector3& point);
    static Vector3 ClosestPoint(const Plane& plane, const Vector3& point);

    Real getA() const;
    Real getB() const;
    Real getC() const;
    Real getD() const;

    Plane<Real>& operator = (const Plane<Real>& plane);
    
protected:
    /* Planar Coeficients */
    Real a, b, c, d;
};

template <typename Real>
Plane<Real>::Plane() {
    this->a = static_cast<Real>(0);
    this->b = static_cast<Real>(1);
    this->c = static_cast<Real>(0);
    this->d = static_cast<Real>(0);
}

template <typename Real>
Plane<Real>::Plane(const Plane& plane) {
    this->a = plane.a;
    this->b = plane.b;
    this->c = plane.c;
    this->d = plane.d;
}

template <typename Real>
Plane<Real>::Plane(Real a, Real b, Real c, Real d) {
    this->a = a;
    this->b = b;
    this->c = c;
    this->d = d;
}

template <typename Real>
Plane<Real>::Plane(const Vector3& normal, Real d) {
    Vector3 normalizedNorma = normal.normalized();
    this->a = normal.x();
    this->b = normal.y();
    this->c = normal.z();
    this->d = d;
}

template <typename Real>
Plane<Real>::Plane(const Vector3& point, const Vector3& normal) {
    Vector3 normalizedNormal = normal.normalized();
    this->a = normal.x();
    this->b = normal.y();
    this->c = normal.z();
    this->d = -point.dot(normal);
}

template <typename Real>
Plane<Real>::Plane(const Vector3& a, const Vector3& b, const Vector3& c) {
    Vector3 normal = Plane<Real>::Normal(a, b, c);
    this->a = normal.x();
    this->b = normal.y();
    this->c = normal.z();
    this->d = -a.dot(normal);
}

template <typename Real>
Plane<Real>::~Plane() {}

template <typename Real>
void Plane<Real>::setA(Real a) {
    this->a = a;
}

template <typename Real>
void Plane<Real>::setB(Real b) {
    this->b = b;
}

template <typename Real>
void Plane<Real>::setC(Real c) {
    this->c = c;
}

template <typename Real>
void Plane<Real>::setD(Real d) {
    this->d = d;
}

template <typename Real>
void Plane<Real>::setNormal(const Vector3& normal) {
    Vector3 normalizedNormal = normal.normalized();
    this->a = normalizedNormal.x();
    this->b = normalizedNormal.y();
    this->c = normalizedNormal.z();
}

template <typename Real>
void Plane<Real>::set(const Vector3& normal, const Vector3& point) {
    Vector3 normalizedNormal = normal.normalized();
    this->a = normalizedNormal.x();
    this->b = normalizedNormal.y();
    this->c = normalizedNormal.z();
    this->d = -point.dot(normalizedNormal);
}

template <typename Real>
void Plane<Real>::set(Real nx, Real ny, Real nz, Real x, Real y, Real z) {
    Vector3 normalizedNormal = Vector3(nx, ny, nz).normalized();
    this->a = normalizedNormal.x();
    this->b = normalizedNormal.y();
    this->c = normalizedNormal.z();
    this->d = -Vector3(x, y, z).dot(normalizedNormal);
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::normal() const {
    return Vector3(this->a, this->b, this->c);
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::origin() const {
    return Vector3(this->a * this->d, this->b * this->d, this->c * this->d);
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::getNormal() const {
    return Vector3(this->a, this->b, this->c);
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::getOrigin() const {
    return Vector3(this->a * this->d, this->b * this->d, this->c * this->d);
}

template <typename Real>
bool Plane<Real>::valid() const {
    Real zero = static_cast<Real>(0);
    if ( this->a == zero && this->b == zero && c == zero ) return false;
    return true;
}

template <typename Real>
bool Plane<Real>::degenerate() const {
    Real zero = static_cast<Real>(0);
    if ( this->a == zero && this->b == zero && c == zero ) return true;
    return false;
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::Normal(const Vector3& a, const Vector3& b, const Vector3& c) {
    return ((b - a).cross(c - a)).normalized();
}

template <typename Real>
bool Plane<Real>::IntersectSphere(const Plane& plane, const Vector3& center, Real radius) {
    if ( radius == static_cast<Real>(0) ) return false;

    //--------------------------------------------------------------------------
    // For a normalized plane (|p.n| = 1), evaluating the plane equation
    // for a point gives the signed distance of the point to the plane
    //--------------------------------------------------------------------------
    Real dist = Vector3::Dot(center, plane.normal()) - plane.d;

    //--------------------------------------------------------------------------
    // If sphere center with +/- radius from plane, plane intersects sphere
    //--------------------------------------------------------------------------
    return std::abs(dist) <= radius;
}

/* Real-Time Collision Detection - Christer Ericson (pg.164)*/
template <typename Real>
bool Plane<Real>::IntersectAABB(const Plane& plane, const Vector3& min, const Vector3& max) {
    if ( min - max == Vector3::Zero() ) return false;

    // These two lines not necessary with a (center, extents) AABB rep.
    Vector3 c = (max + min) * static_cast<Real>(0.5);
    Vector3 e = max - c;
    Vector3 n = plane.normal();

    // Compute the projection interval of b onto L(t) = b.c + t * p.n
    Real r = e[0] * std::abs(n[0]) + e[1] * std::abs(n[1]) * e[2] * std::abs(n[2]);

    // Compute distance of box center from plane
    Real s = Vector3::Dot(n, c) - plane.d;
}

/* Real-Time Collision Detection - Christer Ericson (pg.160)*/
template <typename Real>
bool Plane<Real>::IntersectSegment(const Plane& plane, const Vector3& a, const Vector3& b, Real& t, Vector3& q) {
    //--------------------------------------------------------------------------
    // Compute the t value for the directed line ab intersecting the plane
    //--------------------------------------------------------------------------
    Vector3 ab = b - a;
    Vector3 n = plane.normal();

    t = (plane.d - Vector3::Dot(n, a)) / Vector3::Dot(n, ab);

    //--------------------------------------------------------------------------
    // If t in [0..1] compute and return intersection point
    //--------------------------------------------------------------------------
    if ( t >= static_cast<Real>(0) && t <= static_cast<Real>(1) ) {
        q = a + t * ab;
        return true;
    }

    //--------------------------------------------------------------------------
    // Else no intersection
    //--------------------------------------------------------------------------
    return false;
}

template <typename Real>
inline Real Plane<Real>::Distance(const Plane& plane, const Vector3& point) {
    return (plane.a * point.x() + plane.b * point.y() + plane.c * point.z() + plane.d);
}

template <typename Real>
inline Real Plane<Real>::UnsignedDistance(const Plane& plane, const Vector3& point) {
    return std::abs(Plane<Real>::Distance(point));
}

template <typename Real>
typename Plane<Real>::Vector3 Plane<Real>::ClosestPoint(const Plane& plane, const Vector3& point) {
    return point - (plane.normal() * Plane<Real>::Distance(point));
}

template <typename Real>
inline Real Plane<Real>::getA() const {
    return this->a;
}

template <typename Real>
inline Real Plane<Real>::getB() const {
    return this->b;
}

template <typename Real>
inline Real Plane<Real>::getC() const {
    return this->c;
}

template <typename Real>
inline Real Plane<Real>::getD() const {
    return this->d;
}

template <typename Real>
Plane<Real>& Plane<Real>::operator = (const Plane<Real>& plane) {
    if ( this == &plane ) return *this;

    this->a = plane.a;
    this->b = plane.b;
    this->c = plane.c;
    this->d = plane.d;
    return *this;
}

typedef Plane<double> Planed;
typedef Plane<float> Planef;

}

}

#endif
